/*
 * @Author: qsy
 * @Date: 2024-12-28 14:51:02
 * @LastEditors: qsy
 * @LastEditTime: 2024-12-28 16:00:28
 * @Description: adxl345 驱动
 */
#include <math.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"

#include "board_spi.h"
#include "board_adxl345.h"

static void delayMs(uint32_t time);

void ADXL345_Init(void)
{
	ADXL345_SPI_Init();

    while(ADXL345_GetDeviceId() != 0xE5)
    {
    	printf("ADXL345 Init Fail!\n");
        delayMs(1000);
    }

    ADXL345_WriteReg(DATA_FORMAT, 0x0B);    // 0x0B = 0b00001011
                                           // Bit 3 = 1 (FULL_RES模式)
                                           // Bit 1:0 = 11 (±16g量程)
                                           
    ADXL345_WriteReg(BW_RATE, 0x0F);       // 0x0F = 0b00001111 (3200Hz)
    
    ADXL345_WriteReg(POWER_CTL, 0x08);     // 0x08 = 0b00001000 (测量模式)
    
    ADXL345_WriteReg(INT_ENABLE, 0x80);    

    ADXL345_AutoAdjust();

    printf("ADXL345 Init Success!\n");
}

uint8_t ADXL345_GetDeviceId(void)
{
    uint8_t ret = ADXL345_ReadReg(DEVICE_ID);
    return ret;
}

void ADXL345_ReadXYZ(short *x, short *y, short *z)
{
    uint8_t x0,y0,z0;
    uint8_t x1,y1,z1;

    x0 = ADXL345_ReadReg(DATA_X0);
    y0 = ADXL345_ReadReg(DATA_Y0);
    z0 = ADXL345_ReadReg(DATA_Z0);
    x1 = ADXL345_ReadReg(DATA_X1);
    y1 = ADXL345_ReadReg(DATA_Y1);
    z1 = ADXL345_ReadReg(DATA_Z1);
    // printf("--------------x0:%d,y0:%d,z0:%d\tx1:%d,y1:%d,z1:%d", x0,y0,z0,x1,y1,z1);
    *x = (short)(((uint16_t)x1 << 8) + x0); // DATA_X1为高位有效字节
    *y = (short)(((uint16_t)y1 << 8) + y0); // DATA_Y1为高位有效字节
    *z = (short)(((uint16_t)z1 << 8) + z0); // DATA_Z1为高位有效字节
    // printf("--------------x%d,y%d,z%d",*x,*y,*z);
}

/*读取ADXL345的数据并做滤波处理，读times次再取平均值*/
void ADXL345_ReadAverage(short *x, short *y, short *z, uint8_t times)
{
    if(0 == times)
    {
        return;
    }

    uint8_t i;
    short x_temp,y_temp,z_temp;
    long x_sum = 0;
    long y_sum = 0;
    long z_sum = 0;

    for(i = 0; i < times; i++)
    {
        ADXL345_ReadXYZ(&x_temp, &y_temp, &z_temp);
        x_sum += x_temp;
        y_sum += y_temp;
        z_sum += z_temp;
        delayMs(1);  // 减小延时以提高采样频率
    }
    *x = (short)(x_sum / times);
    *y = (short)(y_sum / times);
    *z = (short)(z_sum / times);
}

/*使用偏移寄存器，进行偏移校准*/
void ADXL345_AutoAdjust(void)
{
    uint8_t i;
    short x_temp,y_temp,z_temp;
    short x_offset = 0;
    short y_offset = 0;
    short z_offset = 0;
    char x_calib = 0;
    char y_calib = 0;
    char z_calib = 0;

    ADXL345_WriteReg(DATA_FORMAT, 0x0B);    // 13位全分辨率，输出数据右对齐，16g量程
    ADXL345_WriteReg(BW_RATE, 0x0A);        // 数据输出速度为100Hz
    ADXL345_WriteReg(POWER_CTL, 0x08);      // 无链接，测量模式
    ADXL345_WriteReg(INT_ENABLE, 0x80);     // DATA_READY中断

    delayMs(12);

    for(i = 0; i < 10; i++)
    {
        ADXL345_ReadAverage(&x_temp, &y_temp, &z_temp, 10);
        x_offset += x_temp;
        y_offset += y_temp;
        z_offset += z_temp;
    }
    x_offset /= 10;
    y_offset /= 10;
    z_offset /= 10;

    x_calib =- x_offset / 4;
    y_calib =- y_offset / 4;
    z_calib =- (z_offset - 256) / 4;
    ADXL345_WriteReg(OFSX, x_calib);
    ADXL345_WriteReg(OFSY, y_calib);
    ADXL345_WriteReg(OFSZ, z_calib);
}

/*计算ADXL345角度，x/y/表示各方向上的加速度分量，direction表示要获得的角度*/
short ADXL345_GetAngle(float x, float y, float z, uint8_t direction)
{
    float temp;
    float res = 0;	                        // 弧度值
    switch(direction)
    {
        case 0:                             // 0表示与Z轴的角度
            temp = sqrt((x*x + y*y)) / z;
            res = atan(temp);
            break;
        case 1:                             // 1表示与X轴的角度
            temp = x / sqrt((y*y + z*z));
            res = atan(temp);
            break;
        case 2:                             // 2表示与Y轴的角度
            temp = y / sqrt((x*x + z*z));
            res = atan(temp);
            break;
    }
    return res * 180 / 3.14;                // 返回角度值
    // return res*180/3.14*10;      //乘以10是为了取一位小数，角度精确到0.1°所以要乘以10
}

/**
 @brief 写寄存器
 @param addr -[in] 寄存器地址
 @param data -[in] 写入数据
 @return 无
*/
void ADXL345_WriteReg(uint8_t addr, uint8_t data)
{
    uint8_t send_data[2];
    uint32_t size = 2;

    addr &= 0x3F;
    send_data[0] = addr;
    send_data[1] = data;

    SPI_CS_LOW;

    ADXL345_SPI_Write(send_data, size);

    SPI_CS_HIGH;
}

/**
 @brief 读寄存器
 @param addr -[in] 寄存器地址
 @return 读出一字节数据
*/
uint8_t ADXL345_ReadReg(uint8_t addr)
{
    uint8_t receive_data;
    uint32_t size = 1;

	addr &= 0x3F;
	addr |= 0x80;

    SPI_CS_LOW;

    ADXL345_SPI_Write(&addr, size);
    ADXL345_SPI_Read(&receive_data, size);

    SPI_CS_HIGH;

    return receive_data;
}

/**
 @brief 毫秒级延时函数
 @param time -[in] 延时时间（毫秒）
 @return 无
*/
static void delayMs(uint32_t time)
{
    vTaskDelay(time / portTICK_PERIOD_MS);
}

static float convert_raw_to_g(short raw_data) {
    // 在全分辨率模式下
    // 灵敏度为 3.9 mg/LSB = 0.0039 g/LSB
    // 转换为 m/s²
    return (raw_data * 0.0039f * 9.8f);
}

static void monitor_task(void *arg)
{
    // ... 其他代码保持不变 ...
}

